/*
 * encoder.c
 * 正交编码器驱动实现
 * 
 * 本文件实现了四个编码器的初始化、状态更新和数据读取功能，
 * 以及四个电机的速度和方向控制功能。
 */

#include "encoder.h"

// 编码器实例 - 存储每个编码器的状态信息
static Encoder_t encoder1 = {0, 0, 0}; // 初始化为计数=0, 方向=0, 上一个状态=0
static Encoder_t encoder2 = {0, 0, 0};
static Encoder_t encoder3 = {0, 0, 0};
static Encoder_t encoder4 = {0, 0, 0};

/*
 * 编码器状态转换表
 * 用于确定根据前一个状态和当前状态，编码器的旋转方向
 * 正转序列: 00->01->11->10->00
 * 反转序列: 00->10->11->01->00
 * 
 * 数组索引为前一个状态和当前状态的组合: (last_state << 2) | new_state
 * 值为方向: 1=正向, -1=反向, 0=无效变化或静止
 */
static const int8_t encoder_states[] = {0, -1, 1, 0, 1, 0, 0, -1, -1, 0, 0, 1, 0, 1, -1, 0};



/**
 * @brief 设置电机速度和方向
 * 
 * @param motor_id 电机ID(1-4)
 * @param speed 速度和方向，范围-100到100，负值表示反转
 */
void Motor_SetSpeed(uint8_t motor_id, int32_t speed) 
{   
    uint32_t temp ;
    if(speed == 0)
    {
        temp = 0;
    }
    temp = (speed*10) - 1;
    switch(motor_id) {
        case 1:
            DL_TimerA_setCaptureCompareValue(PWM_morton_INST, temp, DL_TIMER_CC_0_INDEX);
            break;
        case 2:
            DL_TimerA_setCaptureCompareValue(PWM_morton_INST, temp, DL_TIMER_CC_1_INDEX);
            break;
        case 3:
            DL_TimerA_setCaptureCompareValue(PWM_morton_INST, temp, DL_TIMER_CC_2_INDEX);
            break;
        case 4:
            DL_TimerA_setCaptureCompareValue(PWM_morton_INST, temp, DL_TIMER_CC_3_INDEX);
            break;
    }
    
    // PWM的调整由seed控制
}

/**
 * @brief 编码器初始化
 * 
 * 读取所有编码器的初始状态
 */
void Encoder_Init(void) 
{
    // 读取初始状态
    uint8_t enc1_a = DL_GPIO_readPins(ENCODER1_A_PORT, ENCODER1_A_PIN) ? 1 : 0;
    uint8_t enc1_b = DL_GPIO_readPins(ENCODER1_B_PORT, ENCODER1_B_PIN) ? 1 : 0;
    encoder1.last_state = (enc1_a << 1) | enc1_b; // 组合A和B信号为状态值
    
    uint8_t enc2_a = DL_GPIO_readPins(ENCODER2_A_PORT, ENCODER2_A_PIN) ? 1 : 0;
    uint8_t enc2_b = DL_GPIO_readPins(ENCODER2_B_PORT, ENCODER2_B_PIN) ? 1 : 0;
    encoder2.last_state = (enc2_a << 1) | enc2_b;
    
    uint8_t enc3_a = DL_GPIO_readPins(ENCODER3_A_PORT, ENCODER3_A_PIN) ? 1 : 0;
    uint8_t enc3_b = DL_GPIO_readPins(ENCODER3_B_PORT, ENCODER3_B_PIN) ? 1 : 0;
    encoder3.last_state = (enc3_a << 1) | enc3_b;
    
    uint8_t enc4_a = DL_GPIO_readPins(ENCODER4_A_PORT, ENCODER4_A_PIN) ? 1 : 0;
    uint8_t enc4_b = DL_GPIO_readPins(ENCODER4_B_PORT, ENCODER4_B_PIN) ? 1 : 0;
    encoder4.last_state = (enc4_a << 1) | enc4_b;
}

/**
 * @brief 更新编码器状态
 * 
 * 读取所有编码器的当前状态，并根据状态变化更新计数值和方向
 * 需要在主循环中定期调用
 */
void Encoder_Update(void)
{
    // 编码器1状态更新
    uint8_t enc1_a = DL_GPIO_readPins(ENCODER1_A_PORT, ENCODER1_A_PIN) ? 1 : 0;
    uint8_t enc1_b = DL_GPIO_readPins(ENCODER1_B_PORT, ENCODER1_B_PIN) ? 1 : 0;
    uint8_t new_state1 = (enc1_a << 1) | enc1_b; // 组合A和B信号为状态值
    
    if (new_state1 != encoder1.last_state) { // 状态有变化
        uint8_t combined_state = (encoder1.last_state << 2) | new_state1; // 组合上一个状态和当前状态
        int8_t direction = encoder_states[combined_state & 0x0F]; // 查表确定方向
        
        encoder1.count += direction; // 更新计数值
        if (direction != 0) {
            encoder1.direction = direction; // 更新方向
        }
        
        encoder1.last_state = new_state1; // 保存当前状态
    }
    
    // 编码器2状态更新
    uint8_t enc2_a = DL_GPIO_readPins(ENCODER2_A_PORT, ENCODER2_A_PIN) ? 1 : 0;
    uint8_t enc2_b = DL_GPIO_readPins(ENCODER2_B_PORT, ENCODER2_B_PIN) ? 1 : 0;
    uint8_t new_state2 = (enc2_a << 1) | enc2_b;
    
    if (new_state2 != encoder2.last_state) {
        uint8_t combined_state = (encoder2.last_state << 2) | new_state2;
        int8_t direction = encoder_states[combined_state & 0x0F];
        
        encoder2.count += direction;
        if (direction != 0) {
            encoder2.direction = direction;
        }
        
        encoder2.last_state = new_state2;
    }
    
    // 编码器3状态更新
    uint8_t enc3_a = DL_GPIO_readPins(ENCODER3_A_PORT, ENCODER3_A_PIN) ? 1 : 0;
    uint8_t enc3_b = DL_GPIO_readPins(ENCODER3_B_PORT, ENCODER3_B_PIN) ? 1 : 0;
    uint8_t new_state3 = (enc3_a << 1) | enc3_b;
    
    if (new_state3 != encoder3.last_state) {
        uint8_t combined_state = (encoder3.last_state << 2) | new_state3;
        int8_t direction = encoder_states[combined_state & 0x0F];
        
        encoder3.count += direction;
        if (direction != 0) {
            encoder3.direction = direction;
        }
        
        encoder3.last_state = new_state3;
    }
    
    // 编码器4状态更新
    uint8_t enc4_a = DL_GPIO_readPins(ENCODER4_A_PORT, ENCODER4_A_PIN) ? 1 : 0;
    uint8_t enc4_b = DL_GPIO_readPins(ENCODER4_B_PORT, ENCODER4_B_PIN) ? 1 : 0;
    uint8_t new_state4 = (enc4_a << 1) | enc4_b;
    
    if (new_state4 != encoder4.last_state) {
        uint8_t combined_state = (encoder4.last_state << 2) | new_state4;
        int8_t direction = encoder_states[combined_state & 0x0F];
        
        encoder4.count += direction;
        if (direction != 0) {
            encoder4.direction = direction;
        }
        
        encoder4.last_state = new_state4;
    }
}

/**
 * @brief 获取编码器1计数值
 * @return int32_t 编码器1的累积计数值
 */
int32_t Encoder1_GetCount(void) 
{
    return encoder1.count;
}

/**
 * @brief 获取编码器1方向
 * @return int8_t 编码器1的旋转方向(1=正向, -1=反向, 0=静止)
 */
int8_t Encoder1_GetDirection(void) 
{
    return encoder1.direction;
}

/**
 * @brief 获取编码器2计数值
 * @return int32_t 编码器2的累积计数值
 */
int32_t Encoder2_GetCount(void) 
{
    return encoder2.count;
}

/**
 * @brief 获取编码器2方向
 * @return int8_t 编码器2的旋转方向(1=正向, -1=反向, 0=静止)
 */
int8_t Encoder2_GetDirection(void) 
{
    return encoder2.direction;
}

/**
 * @brief 获取编码器3计数值
 * @return int32_t 编码器3的累积计数值
 */
int32_t Encoder3_GetCount(void) 
{
    return encoder3.count;
}

/**
 * @brief 获取编码器3方向
 * @return int8_t 编码器3的旋转方向(1=正向, -1=反向, 0=静止)
 */
int8_t Encoder3_GetDirection(void) 
{
    return encoder3.direction;
}

/**
 * @brief 获取编码器4计数值
 * @return int32_t 编码器4的累积计数值
 */
int32_t Encoder4_GetCount(void) 
{
    return encoder4.count;
}

/**
 * @brief 获取编码器4方向
 * @return int8_t 编码器4的旋转方向(1=正向, -1=反向, 0=静止)
 */
int8_t Encoder4_GetDirection(void) 
{
    return encoder4.direction;
}

/**
 * @brief 重置编码器1计数值
 */
void Encoder1_Reset(void) 
{
    encoder1.count = 0;
}

/**
 * @brief 重置编码器2计数值
 */
void Encoder2_Reset(void) 
{
    encoder2.count = 0;
}

/**
 * @brief 重置编码器3计数值
 */
void Encoder3_Reset(void) 
{
    encoder3.count = 0;
}

/**
 * @brief 重置编码器4计数值
 */
void Encoder4_Reset(void) 
{
    encoder4.count = 0;
}